#! /usr/bin/env python3
'''
author:toamin
function:代码由四部分组成,定位信息+规划算法+控制算法+Ros通信,实现单车点到点规划控制
'''

import sys
sys.path.append('/home/mvccep/MVTP_ws_tm/src/one_car_planning_control/base/')
from mvccep_image_localization.msg import base_localization # 获取定位信息
from geometry_msgs.msg import Twist
import rospy # ros发布订阅用到
import numpy as np # np数学库
# from Parameter import Parameter as PA # A*栅格地图参数、车辆尺寸参数
import hybrid_astar as h_astar # 混合A*算法s
import math
import startP_sub as startP_sub
import matplotlib.pyplot as plt
from PurePursuit import PurePursuitControl 


# 车辆状态量
class State:
    def __init__(self, x, y, yaw, v):
        self.x = x
        self.y = y
        self.yaw = yaw # yaw单位为角度
        self.v = v # 小车当前速度

# 混合A*规划路径，输入：起点、终点（x, y, yaw）State类型； 输出：路径（list类型，每一个元素都是State类型）
def hybridAstarPathPlanning(startP, endP):
    """
    规划算法
    Args:startP (list)、endP (list)
    Returns:path 路径
    """
    # 起点终点预处理，转换为A*地图坐标 （实际场景中以矩形中点为坐标原点，A*地图中以左下角为坐标原点）
    startP[0] = startP[0] / PA.N + PA.MAP_SIZE[0] / 2
    startP[1] = startP[1] / PA.N + PA.MAP_SIZE[0] / 2
    endP[0] = endP[0] / PA.N + PA.MAP_SIZE[0] / 2
    endP[1] = endP[1] / PA.N + PA.MAP_SIZE[0] / 2
    
    # path是一个类实例，包含x, y, yaw, direction, cost属性，其中每个变量均为list
    path = h_astar.main(startP, endP) #调用混合A*进行路径规划
    
    # A*地图坐标转换为实际坐标（相对于预处理公式中的逆转换）
    for i in range(len(path.x)):
        path.x[i] = (path.x[i] - PA.MAP_SIZE[0]/2) * PA.N
        path.y[i] = (path.y[i] - PA.MAP_SIZE[1]/2) * PA.N
    
    return path

class OneCarControl(object):
    """
    path已知,由控制算法得到车辆控制量,随后Ros进行发布
    """
    def __init__(self, path, robot_id):
        self.robot_id = robot_id
        self.path = path
        # 定义订阅者，订阅位置信息，Subscribe类的实例化（话题名， 消息类型， 回调函数）
        self.draw_state = State([],[],[],[])
        self.sub = rospy.Subscriber("base_localization", base_localization, self.controlCallback, queue_size = 30)
        # 定义发布者
        self.pub = rospy.Publisher("/robot_{}/cmd_vel".format(robot_id), Twist, queue_size = 30)
        self.ppcontroller = PurePursuitControl()

    # 回调函数是系统进行传参的，以下msg接收的值是话题/base_localization发布出来的消息
    # (相关命令 rostopic echo /base_localization)
    def controlCallback(self, msg):
        # 从定位拿到小车实时状态
        self.robot_list = [[msg.robot_0.pos.x, msg.robot_0.pos.y, msg.robot_0.angle, msg.robot_0.twist_linear.x], 
                           [msg.robot_1.pos.x, msg.robot_1.pos.y, msg.robot_1.angle, msg.robot_1.twist_linear.x],
                           [msg.robot_2.pos.x, msg.robot_2.pos.y, msg.robot_2.angle, msg.robot_2.twist_linear.x],
                           [msg.robot_3.pos.x, msg.robot_3.pos.y, msg.robot_3.angle, msg.robot_3.twist_linear.x],
                           [msg.robot_4.pos.x, msg.robot_4.pos.y, msg.robot_4.angle, msg.robot_4.twist_linear.x]]
        self.state = State(self.robot_list[self.robot_id][0], self.robot_list[self.robot_id][1], 
                           self.robot_list[self.robot_id][2], self.robot_list[self.robot_id][3])
        
        self.draw_state.x.append(self.state.x)
        self.draw_state.y.append(self.state.y)
        self.draw_state.yaw.append(self.state.yaw)
        self.draw_state.v.append(self.state.v) # 画图用
        
        if not self.ppcontroller.STOP_FLAG:
            # 纯跟踪算法，传入小车实时状态、两个数组(即一系列路径点的二维坐标)，得到每个点对应的方向盘转角           
            vel_msg = Twist() # 定义一条Twist类型的消息
            # 在回调函数中循环地计算目标角速度
            vel_msg.linear.x, vel_msg.angular.z = self.ppcontroller.pure_pursuit_control(self.state, self.path)
            # 发布线速度加速度信息
            self.pub.publish(vel_msg)
            rospy.loginfo("Publish velocity command [%.2f m/s, %.2f rad/s]", vel_msg.linear.x, vel_msg.angular.z)
             
        else:
            # 自动结束ros发布的命令
            rospy.loginfo("*** Arrived the end point! ---------->>")
            rospy.signal_shutdown('Over')
            
def drawPath(draw_state, path):
    plt.title("轨迹对比图")
    plt.plot(draw_state.x, draw_state.y, color = "r", marker = ".", label = "真实轨迹")
    plt.plot(path.x, path.y, color = "b", linewidth = 2, label = "参考轨迹")
    # plt.plot(draw_state.x, draw_state.v, color = "green", label = "速度")
    # plt.plot(path.x, path.direction, color = "y", label = "Direction")
    # 设置横纵坐标的名称以及对应字体格式
    font2 = {'family' : 'Times New Roman', 'weight' : 'normal', 'size' : 12}
    plt.xlabel('x(m)',font2)
    plt.ylabel('y(m)',font2)
    plt.grid() # 显示网格
    plt.legend(loc = 'upper right', fontsize=12)  # 将图例显示在右上方
    plt.axis("equal") # 将 x 轴和 y 轴的比例设置为相等
    plt.show()

def getParam():
    try:
        rospy.get_param('~Flag')
        robot_id = rospy.get_param('~robot_id')
        endP = rospy.get_param('~endP') # 运行的时候通过命令行给定
        endP = endP.split(",")
        endP = [float(endP[0]), float(endP[1]), round(math.radians(float(endP[2])), 2)]
        # 设置endP格式:输入[0.52,0.52,90] -> 转化为[0.52,0.52,1.57] (转成弧度并保留两位小数)
    except KeyError:
        robot_id = 3
        # endP = [0.6, 0.2, math.radians(-135)] # 调试的时候给固定值
        endP = [-0.6, -0.52, math.radians(90)] # 调试的时候给固定值
    except Exception:
        print("请正确输入参数，例如:roslaunch one_car_planning_control one_car_planning_control endP:=0,-0.52,90")
    return robot_id, endP

if __name__ == '__main__':
    try:
        print("*** 开始执行主程序--------------->>\n")
        # 初始化一个节点，anonymous表示是否在节点名后追加随机数以确保节点名的唯一性
        rospy.init_node("planning_control_node", anonymous=False)
        # 获取参数
        robot_id, endP = getParam() # 运行的时候通过命令行给定
        startP = startP_sub.startP_sub(robot_id)# 从定位获取车辆的起始点（当前点）
        print("*** 被控车辆: ", robot_id, "号")
        print("*** 车辆起点: ", startP)
        print("*** 车辆终点: ", endP, "\n")
        
        # # A*规划轨迹
        path = hybridAstarPathPlanning(startP, endP) # 调用混合A*算法，传入起点与终点后规划出轨迹并return
        print(len(path.x))
        # 构建控制器
        pathController = OneCarControl(path, robot_id)
        rospy.spin()
        # 轨迹对比图
        drawPath(pathController.draw_state, path)
        
    except rospy.ROSInterruptException:
        rospy.signal_shutdown("Error!")
